package edu.wpi.first.wpilibj.positioning;

import edu.wpi.first.wpilibj.frc.VirtualField;
import edu.wpi.first.wpilibj.util.ArraySet;
import java.util.Iterator;

/**
 * A class to store the position of something nigh constantly moving, with 
 * methods to update it.
 * @author Dylan Frese
 */
public class PositionHandler {
    
    /**
     * A set of {@link PositionListener}s, to be notified every time the
     * position of the PositionHandler is updated.
     */
    private ArraySet <PositionListener> listeners = new ArraySet <PositionListener>();
    
    private double pitch, roll, yaw, x, y;
    
    private final VirtualField field;
    
    public PositionHandler (final VirtualField f){
        field = f;
    }
    
    public PositionHandler (final VirtualField f, int x, int y){
        this (f);
        this.x = x;
        this.y = y;
    }
    
    public PositionHandler (final VirtualField f, int x, int y, int pitch, int roll, int yaw){
        this (f, x, y);
        this.pitch = pitch;
        this.roll = roll;
        this.yaw = yaw;
    }
    
    /**
     * Update the rotation.
     * @param pitch The new pitch
     * @param roll The new roll
     * @param yaw The new yaw
     */
    public void updateRotation (double pitch, double roll, double yaw){
        this.pitch = pitch;
        this.roll = roll;
        this.yaw = yaw;
        notifyListeners();
    }
    
    /**
     * Update the position and rotation
     * @param pitch The new pitch
     * @param roll The new roll
     * @param yaw The new yaw
     * @param x The new x
     * @param y The new y
     */
    public void updatePosition (double pitch, double roll, double yaw, double x, double y){
        this.pitch = pitch;
        this.roll = roll;
        this.yaw = yaw;
        this.x = x;
        this.y = y;
        notifyListeners();
    }
    
    /**
     * Update just the position
     * @param x The new x
     * @param y The new y
     */
    public void updatePosition (double x, double y){
        this.x = x;
        this.y = y;
        notifyListeners();
    }
    
    /**
     * Get the current position/rotation as {@link Coordinates}, using the specified
     * speed.
     * @param speed The speed with which to construct the coordinates.
     * @return the newly constructed Coordinates
     */
    public Coordinates getAsCoordinates (double speed){
        return new Coordinates(x, y, yaw, pitch, roll, speed);
    }
    
    /**
     * Get the current position/rotation as {@link Coordinates}.
     * @return the newly constructed Coordinates
     */
    public Coordinates getAsCoordinates(){
        return new Coordinates(x, y, yaw, pitch, roll);
    }
    
    /**
     * Add a {@link PositionListener} to be notified each time the position changes.
     * @param pl The PositionListener to add.
     */
    public void addListener (PositionListener pl){
        listeners.add(pl);
    }
    
    /**
     * A convenience method to notify all of the listeners with the current coords.
     */
    private void notifyListeners(){
        for (Iterator<PositionListener> it = listeners.iterator();it.hasNext();){
            it.next().update(pitch, roll, yaw, x, y);
        }
    }

    /**
     * Get the pitch of these coordinates.
     * @return the pitch of these coordinates.
     */
    public double getPitch() {
        return pitch;
    }
    
    /**
     * Get the X-coordinate.
     * @return the X-coordinate.
     */
    public double getX() {
        return x;
    }

    /**
     * Get the Y-coordinate.
     * @return the Y-coordinate.
     */
    public double getY() {
        return y;
    }

    /**
     * Get the yaw of these coordinates.
     * @return the yaw of these coordinates.
     */
    public double getYaw() {
        return yaw;
    }
    
    /**
     * Get the roll of these coordinates.
     * @return the roll of these coordinates.
     */
    public double getRoll() {
        return roll;
    }
    
    /**
     * Detects whether the PositionHandler represents a location on a bridge
     * @return whether or not this is on a bridge.
     */
    public boolean isOnBridge(){
        throw new UnsupportedOperationException("Not supported yet!");
    }
    
}
